'use strict';
import { TF, thetaToQuaternion } from './tf';
import { DATA } from './data';
import { NAV } from './navigation';
var PAINT = PAINT || {
	editContainer: null,
	obstaclePoints: [],

	pointLastPos: null,
	pointGraphics: null,

	lineStartPoint: null,
	lineStartPointShape: null,

	circleCenterPoint: null,
	circleCenterPointShape: null,

	rectStartPoint: null,
	rectStartPointShape: null,

	rubberLastPos: null,
	rubberGraphics: null,

	poseStartPoint: null,
	poseStartPointShape: null,

	move: (event) => {
	},


	//绘图，位置估计
	pose: (event) => {
		var strokeSize = 2;
		var strokeColor = '#00ff00';
		// var graphics = new createjs.Graphics();
    	var pos = {
	    	x: event.stageX,
	    	y: event.stageY
	    };
	    if (!PAINT.poseStartPoint){
				PAINT.poseStartPoint = pos;
				PAINT.editContainer = new createjs.Container();
				PAINT.editContainer.name = 'poseEstimate';
				// draw start point
				var graphics = new createjs.Graphics();
				graphics.beginFill('#00ff00');
				graphics.drawCircle(pos.x, pos.y, 5);
				PAINT.poseStartPointShape = new createjs.Shape(graphics);
				DATA.stage.stage.addChild(PAINT.editContainer, PAINT.poseStartPointShape);
	    }else{
	    	var poseGraphics = new createjs.Graphics();
	    	poseGraphics.setStrokeStyle(strokeSize);
    		poseGraphics.beginStroke(strokeColor);
	    	poseGraphics.moveTo(PAINT.poseStartPoint.x, PAINT.poseStartPoint.y);
	    	poseGraphics.lineTo(pos.x, pos.y);
	    	// convert to map 
	    	var startRos = TF.pxToRos(PAINT.poseStartPoint);
	    	var endRos = TF.pxToRos(pos);
	    	var dis = Math.sqrt(Math.pow((startRos.x-endRos.x),2)+Math.pow((startRos.y-endRos.y),2));
	    	var sin = (endRos.y - startRos.y) / dis;
	    	var cos = (endRos.x - startRos.x) / dis;
	    	var yaw = Math.acos(cos);
	    	if (sin < 0){
	    		yaw = 2 * Math.PI - yaw;		
	    	}
	    	var pose = {
	    		position: {
	    			x: startRos.x,
	    			y: startRos.y,
	    			z: 0
	    		},
	    		orientation: thetaToQuaternion(yaw)
	    	};
	    	NAV.sendInitialPose(pose);
	    	poseGraphics.endStroke();
	    	var shape = new createjs.Shape(poseGraphics);
	    	PAINT.editContainer.addChild(shape);
	    	PAINT.poseStartPoint = null;
	    	DATA.stage.stage.removeChild(shape);
	    	DATA.stage.stage.removeEventListener('click', PAINT.pose);
	    	setTimeout(function(){
	    		// remove pose estimate arrow
	    		DATA.stage.stage.removeChild(PAINT.editContainer, PAINT.poseStartPointShape);
	    		PAINT.editContainer = null;
	    		PAINT.poseStartPointShape = null;
	    	}, 500);
	    }
	},
};

export { PAINT }

